#include <iostream>
#include <cstdio>
#include <string>

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
/****************************************************************************/
using std::string;
using std::exception;
using std::cout;
using std::cerr;
using std::endl;
using std::vector;
/*****************************************************************************/


int main(int argc, char** argv)
{
    /* code */
    ros::init(argc, argv, "tf2pose");
    ros::NodeHandle n;
    ros::Publisher tf_pose_pub = n.advertise<geometry_msgs::PoseWithCovarianceStamped>("tf_pose", 20); //tf pose publish
   
    tf::TransformListener listener;
    tf::StampedTransform transform;
    geometry_msgs::PoseWithCovarianceStamped tf_pose_msg;


    while(n.ok())
    {
        try
        {
			listener.waitForTransform("/map", "/base_footprint",ros::Time(0), ros::Duration(10.0));
        	listener.lookupTransform("/map", "/base_footprint",ros::Time(0), transform);
        
			tf_pose_msg.header.stamp = ros::Time::now();
			tf_pose_msg.pose.pose.position.x = transform.getOrigin().x();
    		tf_pose_msg.pose.pose.position.y = transform.getOrigin().y();
    		tf_pose_msg.pose.pose.position.z = transform.getOrigin().z();
    		tf_pose_msg.pose.pose.orientation.x = transform.getRotation().getX();
    		tf_pose_msg.pose.pose.orientation.y = transform.getRotation().getY();
    		tf_pose_msg.pose.pose.orientation.z = transform.getRotation().getZ();
        	tf_pose_msg.pose.pose.orientation.w = transform.getRotation().getW();

            //TODO: covariance is empty

			tf_pose_pub.publish(tf_pose_msg);
            ros::Duration(0.1).sleep();
		}

        catch(tf::TransformException &ex)
        {
            ROS_ERROR("%s",ex.what());
        }

        ros::spinOnce();

    }

    return 0;
}


